#include <encoder/aidike_encoder.h>

namespace encoder {

AidikeEncoder::AidikeEncoder(std::shared_ptr<rclcpp::Node> node, const std::string joint_name):
	node_(node),
	raw_value_(0),
	encoder_resolution_(0),
	reduction_ratio_(0.0)
{
	joint_name_ = joint_name;
	timestamp_ = node_->now().seconds();
}

AidikeEncoder::~AidikeEncoder()
{

}

void AidikeEncoder::init()
{
	node_->get_parameter_or(joint_name_ + "/nodeId", node_id_, 0);
	RCLCPP_INFO(node_->get_logger(), "[AidikeEncoder] %s get nodeId %d", joint_name_.c_str(), node_id_);

	node_->get_parameter_or(joint_name_ + "/busname", busname_, std::string("can0"));
	RCLCPP_INFO(node_->get_logger(), "[AidikeEncoder] %s get busname %s", joint_name_.c_str(), busname_.c_str());
	
	node_->get_parameter_or(joint_name_ + "/reduction_ratio", reduction_ratio_, 0.0);
	RCLCPP_INFO(node_->get_logger(), "[AidikeEncoder] %s get reduction_ratio %.6f", joint_name_.c_str(), reduction_ratio_);

	node_->get_parameter_or(joint_name_ + "/encoder_resolution", encoder_resolution_, 0);
	RCLCPP_INFO(node_->get_logger(), "[AidikeEncoder] %s get encoder_resolution %d", joint_name_.c_str(), encoder_resolution_);

	if (node_id_ == 0) {
		RCLCPP_ERROR(node_->get_logger(), "[AidikeEncoder] bad node id");
		return;
	}


	enc_sub_ = node_->create_subscription<std_msgs::msg::UInt32>(
		"/" + busname_ + "/device" + std::to_string(node_id_) + "/get_entry_600400", 
		10, 
		std::bind(&AidikeEncoder::enc_sub_callback, this, std::placeholders::_1));
	timestamp_sub_ = node_->create_subscription<std_msgs::msg::Float64>(
		"/" + busname_ + "/device" + std::to_string(node_id_) + "/get_entry_600400/timestamp", 
		10, 
		std::bind(&AidikeEncoder::timestamp_sub_callback, this, std::placeholders::_1));
}

uint64_t AidikeEncoder::getValue()
{
	return (uint64_t)raw_value_;
}

double AidikeEncoder::getRadValue()
{	
	double pos = 0.0;
	if (reduction_ratio_ < 1e-6 || !encoder_resolution_)
    {
        return 0.0;
    }

    const double encTorad = encoder_resolution_ * reduction_ratio_ / 2 / M_PI;
	pos = raw_value_ / encTorad;
	return pos;
}

int64_t AidikeEncoder::getTurns()
{
	return 0;
}

void AidikeEncoder::enc_sub_callback(const std_msgs::msg::UInt32::ConstSharedPtr& msg)
{
	raw_value_ = msg->data;
}

void AidikeEncoder::timestamp_sub_callback(const std_msgs::msg::Float64::ConstSharedPtr& msg)
{
	timestamp_ = msg->data;
}

}